#include "NccTracker.h"

NccTracker::NccTracker():
    use_fb_(true),
    normalize_(true),
{

}

/*
 * NccTracker constructor with parameters
 */
NccTracker::NccTracker(int patch_size, Matrix3d& prev_R_CG, Matrix3d& cur_R_CG, Vector3d& trans):
    use_fb_(true),
    normalize_(true),
    patch_size_(patch_size),
    prev_R_CG_(prev_R_CG),
    cur_R_CG_(cur_R_CG),
    t_C1C2_(trans)
{

}

/*
 * NccTracker deconstructor
 */
NccTracker::~NccTracker()
{

}

/*
 * Track function track the image feature with NCC method
 * The image should be rotated to conical orientation first
 * input:
 *      prev_img: previous frame image
 *      cur_img: current frame image
 *      prev_features: previous frame features
 *      cur_features: current frame features
 *      track_status: tracking state, true for ok, false for failure
 */
void NccTracker::Track(Mat &prev_img, Mat &cur_img, vector<Vector2d> &prev_features, vector<Vector2d> &cur_features,
                       vector<bool> &track_status)
{
    if(prev_img.empty() || cur_img.empty())
        return;

    // current features vector
    cur_features.reserve(prev_features.size());
    // tracking feature status
    track_status.reserve(cur_features.size());

    //
    for(auto fts: prev_features)
    {
        // extract the patch in the previous frame
        Mat patch = extractPatch(prev_img, fts, prev_R_CG_);

        // search the features in the current frame

    }
}

/*
 * set the global gravirity value
 */
void NccTracker::setGravirity(Vector3d &gravirity)
{
    gravirity_ = gravirity;
}

/*
 * extract the patch around the feature
 */
Mat NccTracker::extractPatch(Mat &img, Vector2d &feature, Matrix3d& rot_mat)
{
    Matrix3d R_CG = rot_mat;
    // translate the gravirity to camera frame
    Vector3d gravirity_in_camera = R_CG*gravirity_;

    // project the gravirity to image plane
    Vector2d gravirity_in_image = camera_ptr_->h(gravirity_in_camera);

    // normalize the gravirity projection value
    Vector2d gravirity_projection_normalized = gravirity_in_image.normalize();

    // y axis
    Vector2d y_axis(0,1);

    // dot product
    double theta = acos(y_axis.transpose()*gravirity_projection_normalized);

    //
    Point2f center(feature(0), feature(1));
    Mat rot_mat = getRotationMatrix2D(center, theta, 1.0f);

    // warp the image
    Mat dst;
    warpAffine(img, dst, rot_mat, img.size(), INTER_LINEAR);

    // extract a small patch
    return dst;
}

/*
 * search the feature in the epipolar line near
 *
 */
//TODO: check search the epipolar line only need the Rotation matrix, or the translation is also needed
void NccTracker::searchEpipolarLine(Mat &img_patch, Mat &dst_img, Vector2d &cur_ft)
{
    Matrix3d ;
}





